﻿/*
 Wrap Interface of Kinect2
 According to MS Kinect SDK Vesrion 2.0.1410.19000

 Date : 2014/12/3
 Version : 0.5
 
The MIT License (MIT)

Copyright (c) 2014 DavidWTF

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Runtime.InteropServices;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices.ComTypes;

namespace Kinect2
{
    namespace Fusion
    {
        using HRESULT = System.Int32;

        public class CameraParameters
        {
            NUI_FUSION_CAMERA_PARAMETERS cameraParameters = new NUI_FUSION_CAMERA_PARAMETERS();
            static CameraParameters _default = new CameraParameters(0, 0, 0, 0);
            public CameraParameters(float focalLengthX, float focalLengthY, float principalPointX, float principalPointY)
            {
                cameraParameters.focalLengthX = focalLengthX;
                cameraParameters.focalLengthY = focalLengthY;
                cameraParameters.principalPointX = principalPointX;
                cameraParameters.principalPointY = principalPointY;
            }
            public CameraParameters(NUI_FUSION_CAMERA_PARAMETERS cameraParameters)
            {
                this.cameraParameters = cameraParameters;
            }
            public NUI_FUSION_CAMERA_PARAMETERS NUI_FUSION_CAMERA_PARAMETERS { get { return cameraParameters; } }
            public float FocalLengthX { get { return cameraParameters.focalLengthX; } }
            public float FocalLengthY { get { return cameraParameters.focalLengthY; } }
            public float PrincipalPointX { get { return cameraParameters.principalPointX; } }
            public float PrincipalPointY { get { return cameraParameters.principalPointY; } }
            public static CameraParameters Defaults { get { return _default; } }
            public bool Equals(CameraParameters other)
            {
                if (FocalLengthX != other.FocalLengthX
                    || FocalLengthY != other.FocalLengthY
                    || PrincipalPointX != other.PrincipalPointX
                    || PrincipalPointY != other.PrincipalPointY)
                    return false;
                return true;
            }
        }

        public class CameraPoseFinderParameters
        {
            NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS cameraPoseFinderParameters = new NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS();
            static CameraPoseFinderParameters _default = new CameraPoseFinderParameters(0, 0, 0.0f);
            public CameraPoseFinderParameters(int featureSampleLocationsPerFrameCount, int maxPoseHistoryCount, float maxDepthThreshold)
            {
                cameraPoseFinderParameters.featureSampleLocationsPerFrameCount = (uint)featureSampleLocationsPerFrameCount;
                cameraPoseFinderParameters.maxPoseHistoryCount = (uint)maxPoseHistoryCount;
                cameraPoseFinderParameters.maxDepthThreshold = maxDepthThreshold;
            }
            public CameraPoseFinderParameters(NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS cameraPoseFinderParameters)
            {
                this.cameraPoseFinderParameters = cameraPoseFinderParameters;
            }
            public NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS { get { return cameraPoseFinderParameters; } }
            public int FeatureSampleLocationsPerFrame { get { return (int)cameraPoseFinderParameters.featureSampleLocationsPerFrameCount; } }
            public int MaxPoseHistory { get { return (int)cameraPoseFinderParameters.maxPoseHistoryCount; } }
            public float MaxDepthThreshold { get { return cameraPoseFinderParameters.maxDepthThreshold; } }
            public static CameraPoseFinderParameters Defaults { get { return _default; } }
        }

        public class ReconstructionParameters
        {
            NUI_FUSION_RECONSTRUCTION_PARAMETERS reconstructionParameters = new NUI_FUSION_RECONSTRUCTION_PARAMETERS();
            public ReconstructionParameters(float voxelsPerMeter, int voxelCountX, int voxelCountY, int voxelCountZ)
            {
                reconstructionParameters.voxelsPerMeter = voxelsPerMeter;
                reconstructionParameters.voxelCountX = (uint)voxelCountX;
                reconstructionParameters.voxelCountY = (uint)voxelCountY;
                reconstructionParameters.voxelCountZ = (uint)voxelCountZ;
            }
            public ReconstructionParameters(NUI_FUSION_RECONSTRUCTION_PARAMETERS reconstructionParameters)
            {
                this.reconstructionParameters = reconstructionParameters;
            }
            public NUI_FUSION_RECONSTRUCTION_PARAMETERS NUI_FUSION_RECONSTRUCTION_PARAMETERS { get { return reconstructionParameters; } }
            public float VoxelsPerMeter { get { return reconstructionParameters.voxelsPerMeter; } }
            public int VoxelCountX { get { return (int)reconstructionParameters.voxelCountX; } }
            public int VoxelCountY { get { return (int)reconstructionParameters.voxelCountY; } }
            public int VoxelCountZ { get { return (int)reconstructionParameters.voxelCountZ; } }
            public bool Equals(ReconstructionParameters other)
            {
                if (VoxelsPerMeter != other.VoxelsPerMeter
                    || VoxelCountX != other.VoxelCountX
                    || VoxelCountY != other.VoxelCountY
                    || VoxelCountZ != other.VoxelCountZ)
                    return false;
                return true;
            }

        }

        public class FusionImageFrame
        {
            protected NUI_FUSION_IMAGE_FRAME imageFrame = new NUI_FUSION_IMAGE_FRAME();
            protected int bytesPerPixel = 1;
            public NUI_FUSION_IMAGE_FRAME NUI_FUSION_IMAGE_FRAME { get { return imageFrame; } }
            public virtual int BytesPerPixel { get { return bytesPerPixel; } set { bytesPerPixel = value; } }
            public CameraParameters CameraParameters { get { return new CameraParameters(imageFrame.cameraParameters); } }
            public int Height { get { return (int)imageFrame.height; } }
            public int Width { get { return (int)imageFrame.width; } }
            public int PixelDataLength { get { return (int)(imageFrame.width * imageFrame.height); } }
            public FusionImageType ImageType { get { return (FusionImageType)imageFrame.imageType; } }
        }

        public class FusionFloatImageFrame : FusionImageFrame
        {
            public FusionFloatImageFrame(int width, int height)
            {
                imageFrame.width = (uint)width;
                imageFrame.height = (uint)height;
                imageFrame.cameraParameters = CameraParameters.Defaults.NUI_FUSION_CAMERA_PARAMETERS;
                imageFrame.frameBuffer.Pitch = (uint)(width * sizeof(float));
                imageFrame.frameBuffer.pBits = Marshal.AllocCoTaskMem((int)imageFrame.frameBuffer.Pitch * height);
            }
            public FusionFloatImageFrame(int width, int height, CameraParameters cameraParameters)
            {
                imageFrame.width = (uint)width;
                imageFrame.height = (uint)height;
                imageFrame.cameraParameters = cameraParameters.NUI_FUSION_CAMERA_PARAMETERS;
                imageFrame.frameBuffer.Pitch = (uint)(width * sizeof(float));
                imageFrame.frameBuffer.pBits = Marshal.AllocCoTaskMem((int)imageFrame.frameBuffer.Pitch * height);
            }
            public void CopyPixelDataFrom(float[] sourcePixelData)
            {
                Marshal.Copy(sourcePixelData, 0, imageFrame.frameBuffer.pBits, sourcePixelData.Length);
            }
            public void CopyPixelDataTo(float[] destinationPixelData)
            {
                Marshal.Copy(imageFrame.frameBuffer.pBits, destinationPixelData, 0, destinationPixelData.Length);
            }
            public override int BytesPerPixel { get { return sizeof(float); } }

        }

        public class FusionColorImageFrame : FusionImageFrame
        {
            public FusionColorImageFrame(int width, int height)
            {
                imageFrame.width = (uint)width;
                imageFrame.height = (uint)height;
                imageFrame.cameraParameters = CameraParameters.Defaults.NUI_FUSION_CAMERA_PARAMETERS;
                imageFrame.frameBuffer.Pitch = (uint)(width * sizeof(int));
                imageFrame.frameBuffer.pBits = Marshal.AllocCoTaskMem((int)imageFrame.frameBuffer.Pitch * height);
            }
            public FusionColorImageFrame(int width, int height, CameraParameters cameraParameters)
            {
                imageFrame.width = (uint)width;
                imageFrame.height = (uint)height;
                imageFrame.cameraParameters = cameraParameters.NUI_FUSION_CAMERA_PARAMETERS;
                imageFrame.frameBuffer.Pitch = (uint)(width * sizeof(float));
                imageFrame.frameBuffer.pBits = Marshal.AllocCoTaskMem((int)imageFrame.frameBuffer.Pitch * height);
            }
            public void CopyPixelDataFrom(int[] sourcePixelData)
            {
                Marshal.Copy(sourcePixelData, 0, imageFrame.frameBuffer.pBits, sourcePixelData.Length);
            }
            public void CopyPixelDataTo(int[] destinationPixelData)
            {
                Marshal.Copy(imageFrame.frameBuffer.pBits, destinationPixelData, 0, destinationPixelData.Length);
            }
            public override int BytesPerPixel { get { return sizeof(int); } }

        }

        public class FusionPointCloudImageFrame : FusionImageFrame
        {
            public FusionPointCloudImageFrame(int width, int height)
            {
                imageFrame.width = (uint)width;
                imageFrame.height = (uint)height;
                imageFrame.cameraParameters = CameraParameters.Defaults.NUI_FUSION_CAMERA_PARAMETERS;
                imageFrame.frameBuffer.Pitch = (uint)(width * sizeof(float));
                imageFrame.frameBuffer.pBits = Marshal.AllocCoTaskMem((int)imageFrame.frameBuffer.Pitch * height);
            }
            public FusionPointCloudImageFrame(int width, int height, CameraParameters cameraParameters)
            {
                imageFrame.width = (uint)width;
                imageFrame.height = (uint)height;
                imageFrame.cameraParameters = cameraParameters.NUI_FUSION_CAMERA_PARAMETERS;
                imageFrame.frameBuffer.Pitch = (uint)(width * sizeof(float));
                imageFrame.frameBuffer.pBits = Marshal.AllocCoTaskMem((int)imageFrame.frameBuffer.Pitch * height);
            }
            public void CopyPixelDataFrom(float[] sourcePixelData)
            {
                Marshal.Copy(sourcePixelData, 0, imageFrame.frameBuffer.pBits, sourcePixelData.Length);
            }
            public void CopyPixelDataTo(float[] destinationPixelData)
            {
                Marshal.Copy(imageFrame.frameBuffer.pBits, destinationPixelData, 0, destinationPixelData.Length);
            }
            public override int BytesPerPixel { get { return sizeof(float); } }

        }

        public class CameraPoseFinder : BaseInterfaceWrap<INuiFusionCameraPoseFinder>
        {
            public CameraPoseFinder() : base() { }
            internal CameraPoseFinder(INuiFusionCameraPoseFinder arg) : base(arg) { }
            public const float DefaultMinimumDistanceThreshold = 0.0f;
            public static CameraPoseFinder FusionCreateCameraPoseFinder(CameraPoseFinderParameters cameraPoseFinderParameters, int randomFeatureLocationAndThresholdSeed)
            {
                IntPtr randomFeatureLocationAndThresholdSeed2 = Marshal.AllocCoTaskMem(sizeof(int));
                try
                {
                    NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS cameraPoseFinderParameters2 = cameraPoseFinderParameters.NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS;
                    Marshal.WriteInt32(randomFeatureLocationAndThresholdSeed2, randomFeatureLocationAndThresholdSeed);

                    INuiFusionCameraPoseFinder nuiFusionCameraPoseFinder;
                    KinectFusion.NuiFusionCreateCameraPoseFinder(ref cameraPoseFinderParameters2, randomFeatureLocationAndThresholdSeed2, out nuiFusionCameraPoseFinder);
                    return new CameraPoseFinder(nuiFusionCameraPoseFinder);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(randomFeatureLocationAndThresholdSeed2);
                }
                return null;
            }
            public static CameraPoseFinder FusionCreateCameraPoseFinder(CameraPoseFinderParameters cameraPoseFinderParameters)
            {
                try
                {
                    NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS cameraPoseFinderParameters2 = cameraPoseFinderParameters.NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS;
                    INuiFusionCameraPoseFinder nuiFusionCameraPoseFinder;
                    KinectFusion.NuiFusionCreateCameraPoseFinder(ref cameraPoseFinderParameters2, IntPtr.Zero, out nuiFusionCameraPoseFinder);
                    return new CameraPoseFinder(nuiFusionCameraPoseFinder);
                }
                catch { }
                return null;
            }
            public MatchCandidates FindCameraPose(FusionFloatImageFrame depthFloatFrame, FusionColorImageFrame colorFrame)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME colorFrame2 = colorFrame.NUI_FUSION_IMAGE_FRAME;
                    INuiFusionMatchCandidates matchCandidates = null;
                    origin.FindCameraPose(ref depthFloatFrame2, ref colorFrame2, out matchCandidates);
                    return new MatchCandidates(matchCandidates);
                }
                catch { }
                return null;
            }
            public CameraPoseFinderParameters GetCameraPoseFinderParameters()
            {
                try
                {
                    NUI_FUSION_CAMERA_POSE_FINDER_PARAMETERS cameraPoseFinderParameters;
                    origin.GetCameraPoseFinderParameters(out cameraPoseFinderParameters);
                    return new CameraPoseFinderParameters((int)cameraPoseFinderParameters.featureSampleLocationsPerFrameCount,
                        (int)cameraPoseFinderParameters.maxPoseHistoryCount,
                        cameraPoseFinderParameters.maxDepthThreshold);
                }
                catch { }
                return CameraPoseFinderParameters.Defaults;
            }
            public void ProcessFrame(FusionFloatImageFrame depthFloatFrame, FusionColorImageFrame colorFrame, Matrix4 worldToCameraTransform, float minimumDistanceThreshold, out bool addedPose, out bool trimmedHistory)
            {
                addedPose = false;
                trimmedHistory = false;
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME colorFrame2 = colorFrame.NUI_FUSION_IMAGE_FRAME;
                    Matrix4 worldToCameraTransform2 = worldToCameraTransform;
                    origin.ProcessFrame(ref depthFloatFrame2, ref colorFrame2, ref worldToCameraTransform2, minimumDistanceThreshold, out addedPose, out trimmedHistory);
                }
                catch { }
            }
            public int GetStoredPoseCount()
            {
                try { return (int)origin.GetStoredPoseCount(); }
                catch { }
                return 0;
            }
            public void ResetCameraPoseFinder()
            {
                try { origin.ResetCameraPoseFinder(); }
                catch { }
            }
            public void LoadCameraPoseFinderDatabase(string fileName)
            {
                try { origin.LoadCameraPoseFinderDatabase(fileName); }
                catch { }
            }
            public void SaveCameraPoseFinderDatabase(string fileName)
            {
                try { origin.SaveCameraPoseFinderDatabase(fileName); }
                catch { }
            }
        }

        public class MatchCandidates : BaseInterfaceWrap<INuiFusionMatchCandidates>
        {
            public MatchCandidates() : base() { }
            internal MatchCandidates(INuiFusionMatchCandidates arg) : base(arg) { }
            public float CalculateMinimumDistance()
            {
                try
                {
                    float minimumDistance;
                    origin.CalculateMinimumDistance(out minimumDistance);
                    return minimumDistance;
                }
                catch { }
                return 0.0f;
            }
            public int GetSimilarityCount()
            {
                try { return (int)origin.MatchSimilarityCount(); }
                catch { }
                return 0;
            }
            public int GetPoseCount()
            {
                try { return (int)origin.MatchPoseCount(); }
                catch { }
                return 0;
            }
            public ReadOnlyCollection<float> GetMatchSimilarities()
            {
                try
                {
                    uint count = origin.MatchSimilarityCount();
                    List<float> list = new List<float>((int)count);
                    IntPtr similarityMeasurements;
                    float[] similarityMeasurements2 = new float[(int)count];
                    origin.GetMatchSimilarities(out similarityMeasurements);
                    Marshal.Copy(similarityMeasurements, similarityMeasurements2, 0, (int)count);
                    for (uint i = 0; i < count; i++)
                        list.Add(similarityMeasurements2[i]);
                    return new ReadOnlyCollection<float>(list);
                }
                catch { }
                return null;
            }
            public ReadOnlyCollection<Matrix4> GetMatchPoses()
            {
                try
                {
                    uint count = origin.MatchPoseCount();
                    List<Matrix4> list = new List<Matrix4>((int)count);
                    IntPtr poses;
                    origin.GetMatchPoses(out poses);
                    for (uint i = 0; i < count; i++)
                    {
                        IntPtr ptr = new IntPtr(poses.ToInt64() + i * Marshal.SizeOf(typeof(Matrix4)));
                        Matrix4 pose = (Matrix4)Marshal.PtrToStructure(ptr, typeof(Matrix4));
                        list.Add(pose);
                    }
                    return new ReadOnlyCollection<Matrix4>(list);
                }
                catch { }
                return null;
            }
        }

        public class ColorMesh : BaseInterfaceWrap<INuiFusionColorMesh>
        {
            public ColorMesh() : base() { }
            internal ColorMesh(INuiFusionColorMesh arg) : base(arg) { }
            public ReadOnlyCollection<int> GetColors()
            {
                try
                {
                    uint count = origin.ColorCount();
                    List<int> list = new List<int>((int)count);
                    IntPtr colors;
                    int[] colors2 = new int[(int)count];
                    origin.GetColors(out colors);
                    Marshal.Copy(colors, colors2, 0, (int)count);
                    for (uint i = 0; i < count; i++)
                        list.Add(colors2[i]);
                    return new ReadOnlyCollection<int>(list);
                }
                catch { }
                return null;
            }
            public ReadOnlyCollection<int> GetTriangleIndexes()
            {
                try
                {
                    uint count = origin.TriangleVertexIndexCount();
                    List<int> list = new List<int>((int)count);
                    IntPtr triangleVertexIndeices;
                    int[] triangleVertexIndeices2 = new int[(int)count];
                    origin.GetTriangleIndices(out triangleVertexIndeices);
                    Marshal.Copy(triangleVertexIndeices, triangleVertexIndeices2, 0, (int)count);
                    for (uint i = 0; i < count; i++)
                        list.Add(triangleVertexIndeices2[i]);
                    return new ReadOnlyCollection<int>(list);
                }
                catch { }
                return null;
            }
            public ReadOnlyCollection<Vector3> GetNormals()
            {
                try
                {
                    uint count = origin.NormalCount();
                    List<Vector3> list = new List<Vector3>((int)count);
                    IntPtr normals;
                    origin.GetNormals(out normals);
                    int size = Marshal.SizeOf(typeof(Vector3));
                    for (uint i = 0; i < count; i++)
                    {
                        IntPtr ptr = new IntPtr(normals.ToInt64() + i * size);
                        Vector3 normal = (Vector3)Marshal.PtrToStructure(ptr, typeof(Vector3));
                        list.Add(normal);
                    }
                    return new ReadOnlyCollection<Vector3>(list);
                }
                catch { }
                return null;
            }
            public ReadOnlyCollection<Vector3> GetVertices()
            {
                try
                {
                    uint count = origin.VertexCount();
                    List<Vector3> list = new List<Vector3>((int)count);
                    IntPtr vertices;
                    origin.GetVertices(out vertices);
                    int size = Marshal.SizeOf(typeof(Vector3));
                    for (uint i = 0; i < count; i++)
                    {
                        IntPtr ptr = new IntPtr(vertices.ToInt64() + i * size);
                        Vector3 vertex = (Vector3)Marshal.PtrToStructure(ptr, typeof(Vector3));
                        list.Add(vertex);
                    }
                    return new ReadOnlyCollection<Vector3>(list);
                }
                catch { }
                return null;
            }
        }

        public class ColorReconstruction : BaseInterfaceWrap<INuiFusionColorReconstruction>
        {
            public ColorReconstruction() : base() { }
            internal ColorReconstruction(INuiFusionColorReconstruction arg) : base(arg) { }
            public static ColorReconstruction FusionCreateReconstruction(ReconstructionParameters reconstructionParameters, ReconstructionProcessor reconstructionProcessorType, int deviceIndex, Matrix4 initialWorldToCameraTransform)
            {
                try
                {
                    NUI_FUSION_RECONSTRUCTION_PARAMETERS reconstructionParameters2 = reconstructionParameters.NUI_FUSION_RECONSTRUCTION_PARAMETERS;
                    INuiFusionColorReconstruction nuiFusionColorReconstruction = null;
                    KinectFusion.NuiFusionCreateColorReconstruction(ref reconstructionParameters2, (NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE)reconstructionProcessorType, deviceIndex, ref initialWorldToCameraTransform, out nuiFusionColorReconstruction);
                    return new ColorReconstruction(nuiFusionColorReconstruction);
                }
                catch { }
                return null;
            }
            public bool AlignDepthFloatToReconstruction(FusionFloatImageFrame depthFloatFrame, int maxAlignIterationCount, FusionFloatImageFrame deltaFromReferenceFrame, out float alignmentEnergy, Matrix4 worldToCameraTransform)
            {
                alignmentEnergy = 0.0f;
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME deltaFromReferenceFrame2 = deltaFromReferenceFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.AlignDepthFloatToReconstruction(ref depthFloatFrame2, (ushort)maxAlignIterationCount, ref deltaFromReferenceFrame2, out alignmentEnergy, ref worldToCameraTransform);
                    return true;
                }
                catch { }
                return false;
            }
            public bool AlignPointClouds(FusionPointCloudImageFrame referencePointCloudFrame, FusionPointCloudImageFrame observedPointCloudFrame, int maxAlignIterationCount, FusionColorImageFrame deltaFromReferenceFrame, out float alignmentEnergy, ref Matrix4 referenceToObservedTransform)
            {
                alignmentEnergy = 0.0f;
                try
                {
                    NUI_FUSION_IMAGE_FRAME referencePointCloudFrame2 = referencePointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME observedPointCloudFrame2 = observedPointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME deltaFromReferenceFrame2 = deltaFromReferenceFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.AlignPointClouds(ref referencePointCloudFrame2, ref observedPointCloudFrame2, (ushort)maxAlignIterationCount, ref deltaFromReferenceFrame2, out alignmentEnergy, ref referenceToObservedTransform);
                    return true;
                }
                catch { }
                return false;
            }
            public ColorMesh CalculateMesh(int voxelStep)
            {
                try
                {
                    INuiFusionColorMesh mesh = null;
                    origin.CalculateMesh((uint)voxelStep, out mesh);
                    return new ColorMesh(mesh);
                }
                catch { }
                return null;
            }
            public void CalculatePointCloud(FusionPointCloudImageFrame pointCloudFrame, FusionColorImageFrame colorFrame, Matrix4 worldToCameraTransform)
            {
                IntPtr colorFrame2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(NUI_FUSION_IMAGE_FRAME)));
                try
                {
                    NUI_FUSION_IMAGE_FRAME pointCloudFrame2 = pointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    Marshal.StructureToPtr(colorFrame.NUI_FUSION_IMAGE_FRAME, colorFrame2, false);
                    origin.CalculatePointCloud(ref pointCloudFrame2, colorFrame2, ref worldToCameraTransform);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(colorFrame2);
                }
            }
            public void CalculatePointCloud(FusionPointCloudImageFrame pointCloudFrame, Matrix4 worldToCameraTransform)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME pointCloudFrame2 = pointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.CalculatePointCloud(ref pointCloudFrame2, IntPtr.Zero, ref worldToCameraTransform);
                }
                catch { }
            }
            void CalculatePointCloudAndDepth(FusionPointCloudImageFrame pointCloudFrame, FusionFloatImageFrame depthFloatFrame, FusionColorImageFrame colorFrame, Matrix4 worldToCameraTransform)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME pointCloudFrame2 = pointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME colorFrame2 = colorFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.CalculatePointCloudAndDepth(ref pointCloudFrame2, ref depthFloatFrame2, ref colorFrame2, ref worldToCameraTransform);
                }
                catch { }
            }
            public void DepthToDepthFloatFrame(ushort[] depthImageData, FusionFloatImageFrame depthFloatFrame, float minDepthClip, float maxDepthClip, bool mirrorDepth)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.DepthToDepthFloatFrame(depthImageData, (uint)depthImageData.Length, ref depthFloatFrame2, minDepthClip, maxDepthClip, mirrorDepth);
                }
                catch { }
            }
            public void ExportVolumeBlock(int sourceOriginX, int sourceOriginY, int sourceOriginZ, int destinationResolutionX, int destinationResolutionY, int destinationResolutionZ, int voxelStep, short[] volumeBlock, int[] colorVolumeBlock)
            {
                try
                {
                    origin.ExportVolumeBlock((uint)sourceOriginX, (uint)sourceOriginY, (uint)sourceOriginZ,
                        (uint)destinationResolutionX, (uint)destinationResolutionY, (uint)destinationResolutionZ,
                        (uint)voxelStep, (uint)volumeBlock.Length, (uint)colorVolumeBlock.Length, volumeBlock, colorVolumeBlock);
                }
                catch { }
            }
            public void ExportVolumeBlock(int sourceOriginX, int sourceOriginY, int sourceOriginZ, int destinationResolutionX, int destinationResolutionY, int destinationResolutionZ, int voxelStep, short[] volumeBlock)
            {
                try
                {
                    origin.ExportVolumeBlock((uint)sourceOriginX, (uint)sourceOriginY, (uint)sourceOriginZ,
                        (uint)destinationResolutionX, (uint)destinationResolutionY, (uint)destinationResolutionZ,
                        (uint)voxelStep, (uint)volumeBlock.Length, (uint)0, volumeBlock, null);
                }
                catch { }
            }
            public Matrix4 GetCurrentWorldToCameraTransform()
            {
                try
                {
                    Matrix4 worldToCameraTransform;
                    origin.GetCurrentWorldToCameraTransform(out worldToCameraTransform);
                    return worldToCameraTransform;
                }
                catch { }
                return new Matrix4();
            }
            public Matrix4 GetCurrentWorldToVolumeTransform()
            {
                try
                {
                    Matrix4 worldToVolumeTransform;
                    origin.GetCurrentWorldToVolumeTransform(out worldToVolumeTransform);
                    return worldToVolumeTransform;
                }
                catch { }
                return new Matrix4();
            }
            public void ImportVolumeBlock(short[] volumeBlock, int[] colorVolumeBlock)
            {
                try
                {
                    origin.ImportVolumeBlock((uint)volumeBlock.Length, (uint)colorVolumeBlock.Length, volumeBlock, colorVolumeBlock);
                }
                catch { }
            }
            public void ImportVolumeBlock(short[] volumeBlock)
            {
                try
                {
                    origin.ImportVolumeBlock((uint)volumeBlock.Length, 0, volumeBlock, null);
                }
                catch { }
            }
            public void IntegrateFrame(FusionFloatImageFrame depthFloatFrame, FusionColorImageFrame colorFrame, int maxIntegrationWeight, float maxColorIntegrationAngle, Matrix4 worldToCameraTransform)
            {
                IntPtr colorFrame2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(NUI_FUSION_IMAGE_FRAME)));
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    Marshal.StructureToPtr(colorFrame.NUI_FUSION_IMAGE_FRAME, colorFrame2, false);
                    origin.IntegrateFrame(ref depthFloatFrame2, colorFrame2, (UInt16)maxIntegrationWeight, maxColorIntegrationAngle, ref worldToCameraTransform);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(colorFrame2);
                }
            }
            public void IntegrateFrame(FusionFloatImageFrame depthFloatFrame, int maxIntegrationWeight, float maxColorIntegrationAngle, Matrix4 worldToCameraTransform)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.IntegrateFrame(ref depthFloatFrame2, IntPtr.Zero, (UInt16)maxIntegrationWeight, maxColorIntegrationAngle, ref worldToCameraTransform);
                }
                catch { }
            }
            public bool ProcessFrame(FusionFloatImageFrame depthFloatFrame, FusionColorImageFrame colorFrame, int maxAlignIterationCount, int maxIntegrationWeight, float maxColorIntegrationAngle, out float alignmentEnergy, Matrix4 worldToCameraTransform)
            {
                alignmentEnergy = 0.0f;
                IntPtr colorFrame2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(NUI_FUSION_IMAGE_FRAME)));
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    Marshal.StructureToPtr(colorFrame.NUI_FUSION_IMAGE_FRAME, colorFrame2, false);
                    origin.ProcessFrame(ref depthFloatFrame2, colorFrame2, (UInt16)maxAlignIterationCount, (UInt16)maxIntegrationWeight, maxColorIntegrationAngle, out alignmentEnergy, ref worldToCameraTransform);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(colorFrame2);
                }
                return false;
            }
            public bool ProcessFrame(FusionFloatImageFrame depthFloatFrame, int maxAlignIterationCount, int maxIntegrationWeight, float maxColorIntegrationAngle, out float alignmentEnergy, Matrix4 worldToCameraTransform)
            {
                alignmentEnergy = 0.0f;
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.ProcessFrame(ref depthFloatFrame2, IntPtr.Zero, (UInt16)maxAlignIterationCount, (UInt16)maxIntegrationWeight, maxColorIntegrationAngle, out alignmentEnergy, ref worldToCameraTransform);
                }
                catch { }
                return false;
            }
            public void ResetReconstruction(Matrix4 initialWorldToCameraTransform, Matrix4 worldToVolumeTransform)
            {
                IntPtr worldToVolumeTransform2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(Matrix4)));
                try
                {
                    Marshal.StructureToPtr(worldToVolumeTransform, worldToVolumeTransform2, false);
                    origin.ResetReconstruction(ref initialWorldToCameraTransform, worldToVolumeTransform2);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(worldToVolumeTransform2);
                }
            }
            public void ResetReconstruction(Matrix4 initialWorldToCameraTransform)
            {
                try
                {
                    origin.ResetReconstruction(ref initialWorldToCameraTransform, IntPtr.Zero);
                }
                catch { }
            }
            public void SetAlignDepthFloatToReconstructionReferenceFrame(FusionFloatImageFrame referenceDepthFloatFrame)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME referenceDepthFloatFrame2 = referenceDepthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.SetAlignDepthFloatToReconstructionReferenceFrame(ref referenceDepthFloatFrame2);
                }
                catch { }
            }
            public void SmoothDepthFloatFrame(FusionFloatImageFrame depthFloatFrame, FusionFloatImageFrame smoothDepthFloatFrame, int kernelWidth, float distanceThreshold)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME smoothDepthFloatFrame2 = smoothDepthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.SmoothDepthFloatFrame(ref depthFloatFrame2, ref smoothDepthFloatFrame2, (uint)kernelWidth, distanceThreshold);
                }
                catch { }
            }
        }

        public class Mesh : BaseInterfaceWrap<INuiFusionMesh>
        {
            public Mesh() : base() { }
            internal Mesh(INuiFusionMesh arg) : base(arg) { }
            public ReadOnlyCollection<int> GetTriangleIndexes()
            {
                try
                {
                    uint count = origin.TriangleVertexIndexCount();
                    List<int> list = new List<int>((int)count);
                    IntPtr triangleVertexIndeices;
                    int[] triangleVertexIndeices2 = new int[(int)count];
                    origin.GetTriangleIndices(out triangleVertexIndeices);
                    Marshal.Copy(triangleVertexIndeices, triangleVertexIndeices2, 0, (int)count);
                    for (uint i = 0; i < count; i++)
                        list.Add(triangleVertexIndeices2[i]);
                    return new ReadOnlyCollection<int>(list);
                }
                catch { }
                return null;
            }
            public ReadOnlyCollection<Vector3> GetNormals()
            {
                try
                {
                    uint count = origin.NormalCount();
                    List<Vector3> list = new List<Vector3>((int)count);
                    IntPtr normals;
                    origin.GetNormals(out normals);
                    int size = Marshal.SizeOf(typeof(Vector3));
                    for (uint i = 0; i < count; i++)
                    {
                        IntPtr ptr = new IntPtr(normals.ToInt64() + i * size);
                        Vector3 normal = (Vector3)Marshal.PtrToStructure(ptr, typeof(Vector3));
                        list.Add(normal);
                    }
                    return new ReadOnlyCollection<Vector3>(list);
                }
                catch { }
                return null;
            }
            public ReadOnlyCollection<Vector3> GetVertices()
            {
                try
                {
                    uint count = origin.VertexCount();
                    List<Vector3> list = new List<Vector3>((int)count);
                    IntPtr vertices;
                    origin.GetVertices(out vertices);
                    int size = Marshal.SizeOf(typeof(Vector3));
                    for (uint i = 0; i < count; i++)
                    {
                        IntPtr ptr = new IntPtr(vertices.ToInt64() + i * size);
                        Vector3 vertex = (Vector3)Marshal.PtrToStructure(ptr, typeof(Vector3));
                        list.Add(vertex);
                    }
                    return new ReadOnlyCollection<Vector3>(list);
                }
                catch { }
                return null;
            }
        }

        public class Reconstruction : BaseInterfaceWrap<INuiFusionReconstruction>
        {
            public Reconstruction() : base() { }
            internal Reconstruction(INuiFusionReconstruction arg) : base(arg) { }
            public static Reconstruction FusionCreateReconstruction(ReconstructionParameters reconstructionParameters, ReconstructionProcessor reconstructionProcessorType, int deviceIndex, Matrix4 initialWorldToCameraTransform)
            {
                try
                {
                    NUI_FUSION_RECONSTRUCTION_PARAMETERS reconstructionParameters2 = reconstructionParameters.NUI_FUSION_RECONSTRUCTION_PARAMETERS;
                    INuiFusionReconstruction nuiFusionReconstruction = null;
                    KinectFusion.NuiFusionCreateReconstruction(ref reconstructionParameters2, (NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE)reconstructionProcessorType, deviceIndex, ref initialWorldToCameraTransform, out nuiFusionReconstruction);
                    return new Reconstruction(nuiFusionReconstruction);
                }
                catch { }
                return null;
            }
            public bool AlignDepthFloatToReconstruction(FusionFloatImageFrame depthFloatFrame, int maxAlignIterationCount, FusionFloatImageFrame deltaFromReferenceFrame, out float alignmentEnergy, Matrix4 worldToCameraTransform)
            {
                alignmentEnergy = 0.0f;
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME deltaFromReferenceFrame2 = deltaFromReferenceFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.AlignDepthFloatToReconstruction(ref depthFloatFrame2, (ushort)maxAlignIterationCount, ref deltaFromReferenceFrame2, out alignmentEnergy, ref worldToCameraTransform);
                    return true;
                }
                catch { }
                return false;
            }
            public bool AlignPointClouds(FusionPointCloudImageFrame referencePointCloudFrame, FusionPointCloudImageFrame observedPointCloudFrame, int maxAlignIterationCount, FusionColorImageFrame deltaFromReferenceFrame, out float alignmentEnergy, ref Matrix4 referenceToObservedTransform)
            {
                alignmentEnergy = 0.0f;
                try
                {
                    NUI_FUSION_IMAGE_FRAME referencePointCloudFrame2 = referencePointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME observedPointCloudFrame2 = observedPointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME deltaFromReferenceFrame2 = deltaFromReferenceFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.AlignPointClouds(ref referencePointCloudFrame2, ref observedPointCloudFrame2, (ushort)maxAlignIterationCount, ref deltaFromReferenceFrame2, out alignmentEnergy, ref referenceToObservedTransform);
                    return true;
                }
                catch { }
                return false;
            }
            public Mesh CalculateMesh(int voxelStep)
            {
                try
                {
                    INuiFusionMesh mesh = null;
                    origin.CalculateMesh((uint)voxelStep, out mesh);
                    return new Mesh(mesh);
                }
                catch { }
                return null;
            }
            public void CalculatePointCloud(FusionPointCloudImageFrame pointCloudFrame, Matrix4 worldToCameraTransform)
            {
                IntPtr colorFrame2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(NUI_FUSION_IMAGE_FRAME)));
                try
                {
                    NUI_FUSION_IMAGE_FRAME pointCloudFrame2 = pointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.CalculatePointCloud(ref pointCloudFrame2, ref worldToCameraTransform);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(colorFrame2);
                }
            }
            void CalculatePointCloudAndDepth(FusionPointCloudImageFrame pointCloudFrame, FusionFloatImageFrame depthFloatFrame, Matrix4 worldToCameraTransform)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME pointCloudFrame2 = pointCloudFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.CalculatePointCloudAndDepth(ref pointCloudFrame2, ref depthFloatFrame2, ref worldToCameraTransform);
                }
                catch { }
            }
            public void DepthToDepthFloatFrame(ushort[] depthImageData, FusionFloatImageFrame depthFloatFrame, float minDepthClip, float maxDepthClip, bool mirrorDepth)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.DepthToDepthFloatFrame(depthImageData, (uint)depthImageData.Length, ref depthFloatFrame2, minDepthClip, maxDepthClip, mirrorDepth);
                }
                catch { }
            }
            public void ExportVolumeBlock(int sourceOriginX, int sourceOriginY, int sourceOriginZ, int destinationResolutionX, int destinationResolutionY, int destinationResolutionZ, int voxelStep, short[] volumeBlock)
            {
                try
                {
                    origin.ExportVolumeBlock((uint)sourceOriginX, (uint)sourceOriginY, (uint)sourceOriginZ,
                        (uint)destinationResolutionX, (uint)destinationResolutionY, (uint)destinationResolutionZ,
                        (uint)voxelStep, (uint)volumeBlock.Length, volumeBlock);
                }
                catch { }
            }
            public Matrix4 GetCurrentWorldToCameraTransform()
            {
                try
                {
                    Matrix4 worldToCameraTransform;
                    origin.GetCurrentWorldToCameraTransform(out worldToCameraTransform);
                    return worldToCameraTransform;
                }
                catch { }
                return new Matrix4();
            }
            public Matrix4 GetCurrentWorldToVolumeTransform()
            {
                try
                {
                    Matrix4 worldToVolumeTransform;
                    origin.GetCurrentWorldToVolumeTransform(out worldToVolumeTransform);
                    return worldToVolumeTransform;
                }
                catch { }
                return new Matrix4();
            }
            public void ImportVolumeBlock(short[] volumeBlock)
            {
                try { origin.ImportVolumeBlock((uint)volumeBlock.Length, volumeBlock); }
                catch { }
            }
            public void IntegrateFrame(FusionFloatImageFrame depthFloatFrame, int maxIntegrationWeight, Matrix4 worldToCameraTransform)
            {
                IntPtr colorFrame2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(NUI_FUSION_IMAGE_FRAME)));
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.IntegrateFrame(ref depthFloatFrame2, (UInt16)maxIntegrationWeight, ref worldToCameraTransform);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(colorFrame2);
                }
            }
            public bool ProcessFrame(FusionFloatImageFrame depthFloatFrame, int maxAlignIterationCount, int maxIntegrationWeight, float maxColorIntegrationAngle, out float alignmentEnergy, Matrix4 worldToCameraTransform)
            {
                alignmentEnergy = 0.0f;
                IntPtr colorFrame2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(NUI_FUSION_IMAGE_FRAME)));
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.ProcessFrame(ref depthFloatFrame2, (UInt16)maxAlignIterationCount, (UInt16)maxIntegrationWeight, out alignmentEnergy, ref worldToCameraTransform);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(colorFrame2);
                }
                return false;
            }
            public void ResetReconstruction(Matrix4 initialWorldToCameraTransform, Matrix4 worldToVolumeTransform)
            {
                IntPtr worldToVolumeTransform2 = Marshal.AllocCoTaskMem(Marshal.SizeOf(typeof(Matrix4)));
                try
                {
                    Marshal.StructureToPtr(worldToVolumeTransform, worldToVolumeTransform2, false);
                    origin.ResetReconstruction(ref initialWorldToCameraTransform, worldToVolumeTransform2);
                }
                catch { }
                finally
                {
                    Marshal.FreeCoTaskMem(worldToVolumeTransform2);
                }
            }
            public void ResetReconstruction(Matrix4 initialWorldToCameraTransform)
            {
                try { origin.ResetReconstruction(ref initialWorldToCameraTransform, IntPtr.Zero); }
                catch { }
            }
            public void SetAlignDepthFloatToReconstructionReferenceFrame(FusionFloatImageFrame referenceDepthFloatFrame)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME referenceDepthFloatFrame2 = referenceDepthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.SetAlignDepthFloatToReconstructionReferenceFrame(ref referenceDepthFloatFrame2);
                }
                catch { }
            }
            public void SmoothDepthFloatFrame(FusionFloatImageFrame depthFloatFrame, FusionFloatImageFrame smoothDepthFloatFrame, int kernelWidth, float distanceThreshold)
            {
                try
                {
                    NUI_FUSION_IMAGE_FRAME depthFloatFrame2 = depthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    NUI_FUSION_IMAGE_FRAME smoothDepthFloatFrame2 = smoothDepthFloatFrame.NUI_FUSION_IMAGE_FRAME;
                    origin.SmoothDepthFloatFrame(ref depthFloatFrame2, ref smoothDepthFloatFrame2, (uint)kernelWidth, distanceThreshold);
                }
                catch { }
            }
        }
    }
}
